﻿#ifndef METRANSFORM_H
#define METRANSFORM_H

#include "matrixd.h"
#include "MePoint.h"

#include <Eigen/Core>
#include <opencv2/core.hpp>

class MeTransform
{
public:
    //@{
    /*
     * 生成旋转矩阵的一种简单方式是把它作为三个基本旋转的序列复合。
     * 关于右手笛卡尔坐标系的x-,y- 和z-轴的旋转分别叫做roll,pitch和yaw旋转。
     * 这里依然用 rx, ry, rz 标识
     * degree 弧度表示的旋转角度
     * m 输出的齐次坐标 4x4
     */
    static Matrix4d rotateX(const double degree);
    static void rotateX(const double degree, Matrix4d *m);
    static Matrix4d rotateY(const double degree);
    static void rotateY(const double degree, Matrix4d *m);
    static Matrix4d rotateZ(const double degree);
    static void rotateZ(const double degree, Matrix4d *m);
    //@}

    /**
     * @brief 将位姿矩阵进行逆运算 (算法见文档)
     * @param mIn 输入位姿矩阵
     * @param mOut 输出矩阵的逆
     */
    static void invertRotMatrix(Matrixd &mIn, Matrixd &mOut);

    //@{
    /**
     *  欧拉角Z-Y-Z转成矩阵
     *  三维坐标 x, y, z
     *  旋转角度依次为 u, v, w 单位是弧度
     *  m 输出的齐次坐标 4x4
     */
    static void eulerAngleZYZToMatrix(
            const double x, const double y, const double z,
            const double u, const double v, const double w,
            Matrixd *m);
    // 通过旋转矩阵计算的欧拉角
    static void eulerAngleZYZToMatrix2(
            const double x, const double y, const double z,
            const double u, const double v, const double w,
            Matrixd *m);
    static void eulerAngleZYZToMatrix(double xyzuvw[6] ,Matrixd *m);
    static void eulerAngleZYZToMatrix(double xyz[3], double uvw[3], Matrixd *m);
    //@}

    //@{
    /**
     *  欧拉角Z-Y-X转成矩阵
     *  三维坐标 x, y, z
     *  旋转角度依次为 u, v, w 单位是弧度
     *  m 输出的齐次坐标 4x4
     */
    static void eulerAngleZYXToMatrix(
            const double x, const double y, const double z,
            const double u, const double v, const double w,
            Matrixd *m);
    // 通过旋转矩阵计算的欧拉角
    static void eulerAngleZYXToMatrix2(
            const double x, const double y, const double z,
            const double u, const double v, const double w,
            Matrixd *m);
    //@}

    //@{
    /**
     * 位姿齐次矩阵转成欧拉角Z-Y-Z
     * m 输出的齐次坐标 4x4
     * 三维坐标 x, y, z
     * 旋转角度依次为 u, v, w 单位是弧度
     */
    static void matrixToEulerAngleZYZ(const Matrixd &m,
            double &x, double &y, double &z,
            double &u, double &v, double &w);
    //@}

    //@{
    /**
     * 位姿齐次矩阵转成欧拉角Z-Y-X
     * m 输出的齐次坐标 4x4
     * 三维坐标 x, y, z
     * 旋转角度依次为 u, v, w 单位是弧度
     * eps为精度误差 (注意：这个取值必须有意义，不是取的越大越好)
     */
    static void matrixToEulerAngleZYX(const Matrixd &m,
            double &x, double &y, double &z,
            double &u, double &v, double &w,
            const double eps=1e-6);
    // 返回两组解
    static void matrixToEulerAngleZYX(const Matrixd &m,
            double &x, double &y, double &z,
            double uvw1[3], double uvw2[3]);
    //@}

    //@{
    /**
     * the three basic linear transformations
     */
    static Matrix4d translate(double x, double y, double z);
    /// angle unit is radian
    static Matrix4d rotate(double angle, double x, double y, double z);
    static void rotate(double angle, double v[3], Matrix4d &m);
    static void rotate(double angle, double x, double y, double z, Matrix4d &m);
    static Matrix4d scale(double x, double y, double z);
    //@}

    /**
     * @brief 从 v1 旋转到v2的矩阵
     * @param v1
     * @param v2
     * @param m 返回的矩阵
     */
    static void rotateBetweenVectors(double v1[3], double v2[3], Matrix4d &m);

    /**
     * @brief 通过两个向量计算旋转矩阵
     * @param v1 起始向量
     * @param v2 目标向量
     * @param rotVec 返回旋转向量
     */
    static void rotationVector(double v1[3], double v2[3], double rotVec[3]);

    /**
     *函数名 : intersectionLinePlane
     *函数功能描述 : 求面，线交点
     *函数参数 : P1 线上点1，  P2 线上点2，  n 平面法向量，  point 面上任意点，   P 交点坐标
     *函数返回值 : true 求得交点， false 面与线平行，没有交点
     */
    static bool intersectionLinePlane(double P1[3], double P2[3], double n[3], double point[3], double outP[]);

    /**
     * @brief 通过起始起始向量和终止向量，计算出旋转向量
     * @param axis1 起始向量，一般都是Z单位向量
     * @param axis2 终止向量，一般是一个面的法向量的反方向
     * @param outRotVec 输入旋转向量
     */
    static void axisesToRotationVector(double axis1[3], double axis2[3], double outRotVec[3]);
    /**
     * @brief 通过起始起始向量和终止向量，计算出旋转轴和向量
     * @param axis1 起始向量，一般都是Z单位向量
     * @param axis2 终止向量，一般是一个面的法向量的反方向
     * @param angle 旋转角度 （rad）
     * @param outRotAxis 旋转轴
     */
    static void axisesToRotationVector(double axis1[3], double axis2[3], double &angle, double outRotAxis[3]);
    /**
     * @brief 通过起始起始向量和终止向量，计算出旋转矩阵
     * @param axis1 起始向量，一般都是Z单位向量
     * @param axis2 终止向量，一般是一个面的法向量的反方向
     * @param outM 计算出需要的旋转矩阵
     * @param angle 旋转角度 （rad）
     * @param outRotAxis 旋转轴
     */
    static void axisesToRotationMatrix(double axis1[3], double axis2[3], Eigen::Matrix3d &outM, double *angle = nullptr, double outRotAxis[3] = nullptr);
    /**
     * @brief 旋转向量转成矩阵
     * x,y,z 姿势的坐标
     * u, v, w 旋转向量
     * @param m 4x4齐次坐标
     */
    static void rotationVectorToMatrix(const double x, const double y, const double z,
                                               const double u, const double v, const double w,
                                               Matrix4d *m);
    static void rotationVectorToMatrix(double pose[6], Matrix4d *m);

    /**
     * @brief 两个坐标系的矩阵计算，采用初始3点坐标到目标3点坐标最后得到一个矩阵
     * @return m 4x4 矩阵
     */
    static Matrix4d rigidTransform3D_old(double sp1[3], double sp2[3], double sp3[3],
        double tp1[3], double tp2[3], double tp3[3]);

    /// This function finds the optimal Rigid/Euclidean transform in 3D space
    /// 参考 http://nghiaho.com/?page_id=671
    /// aka B = M*A
    /// 输入3D点坐标
    /// 返回一个4x4齐次坐标
    static bool rigidTransform3D(std::vector<MePoint3D> &A, std::vector<MePoint3D> &B, Matrix4d &m_AtoB);
    static bool rigidTransform3D(MePoint3D A[], MePoint3D B[], uint size, Matrix4d &m_AtoB);
    static bool rigidTransform3D(std::vector<MePoint3D> &A, std::vector<MePoint3D> &B,
                                 Eigen::Matrix3d &R_AtoB, Eigen::Vector3d &T_AtoB);
    static bool rigidTransform3D(MePoint3D A[], MePoint3D B[], uint size,
                                 Eigen::Matrix3d &R_AtoB, Eigen::Vector3d &T_AtoB);
    static bool rigidTransform3D(std::vector<Eigen::Vector3d> &A, std::vector<Eigen::Vector3d> &B, Matrix4d &m_AtoB);
    static bool rigidTransform3D(Eigen::Vector3d A[], Eigen::Vector3d B[], uint size, Matrix4d &m_AtoB);
    static bool rigidTransform3D(std::vector<cv::Point3d> &A, std::vector<cv::Point3d> &B,
                                 Eigen::Matrix3d &R_AtoB, Eigen::Vector3d &T_AtoB);
    static bool rigidTransform3D(std::vector<cv::Point3d> &A, std::vector<cv::Point3d> &B,
                                 Matrix4d &m_AtoB);
    static bool rigidTransform3D(cv::Point3d A[], cv::Point3d B[], uint size,
                                 Eigen::Matrix3d &R_AtoB, Eigen::Vector3d &T_AtoB);

    /// 特定坐标系B-A的旋转矩阵 A = M*B (仅计算旋转部分)
    /// {B} x, y, z 平移量 对应的A坐标系的3点. 也就是 [dx_B, px_A], [dy_B, py_A],[dz_B, pz_A]
    static void coordBtoARotationMatrix(double p_A[3], double px_A[3], double py_A[3], double pz_A[3],
                double dx_B, double dy_B, double dz_B, Matrix4d &rotMatrix);

    /// 求J6空间的p点坐标
    /// m_J6toB 三个转换矩阵 0 - 初始位置 1 - 运动到位置1, ...
    /// m_Rot_AtoB {A} to {B} 的旋转矩阵
    /// p_A {A} 中三个坐标点
    /// outP_J6 输出位置
    static void calP_J6WithRotMatrix(Matrix4d m_J6toB[3], Matrix4d &mRot_AtoB, double p_A[3][3], double outP_J6[3]);

    /// 特定坐标系B-A的转换矩阵 A = M*B
    ///
    /// 1. {B} x, y, z 平移量 对应的A坐标系的3点. 也就是 [dx_B, px_A], [dy_B, py_A],[dz_B, pz_A]
    /// 2. 旋转，是在C坐标系旋转，C的坐标系原点 用B坐标系表示为 centerRotate_B, xyz方向一致。
    /// pRx_A 对应 angleX (C坐标系下旋转)
    /// pRy_A 对应 angleY (C坐标系下旋转)
    ///
    /// @return 返回B 到 A的旋转矩阵
    static Matrix4d coordTransAtoB(double p_A[3],
        double px_A[3], double py_A[3], double pz_A[3],
        double dx_B, double dy_B, double dz_B,
        double angleX, double angleY, double pRx_A[3], double pRy_A[3], double centerRotate_B[3]);

    /// 计算围绕旋转的一个点，也就是机器人标定tcp位置的算法
    static Eigen::Vector3d calibTcpPoint(std::vector<Eigen::Matrix3d> &R, std::vector<Eigen::Vector3d> &t);

private:
    /// 通过已知{A}坐标系的值，计算{B}空间内的P点的坐标
    /// 2. 旋转，是在C坐标系旋转，C的坐标系原点 用B坐标系表示为 centerRotate_B, xyz方向一致。
    /// px_A 对应 angleX (C坐标系下旋转)
    /// py_A 对应 angleY (C坐标系下旋转)
    /// m_BtoA 里面仅包含旋转矩阵 从B转成A
    /// outP_B 输出坐标
    static void calPBWithPA(double p_A[3], double px_A[3], double py_A[3],
        double angleX, double angleY,
        Matrix4d &m_BtoA, double centerRotate_B[3],
        double outP_B[3]);
};

#endif // METRANSFORM_H
